/*
 * Connect waveguides.
 *
 * Copyright (c) 1993-2010 Xaveer Leijtens
 *
 * This file is part of libMask.
 *
 * libMask is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * libMask is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 * for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with libMask.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "mask.h"

/**
 * Connect two points starting with one arc and connecting to the end point
 * with a straight line. The direction of the arc in each of the starting
 * point is determined by the sign of the radius. When looking in the
 * direction of the point, a positive radius curves to the right and a
 * negative radius curves to the left.
 */
struct geom_al arc_line(double r0, double o0, double x1, double y1, double w)
{
  double m0[2]; /* Middle of circle through first point. */
  double alpha; /* Angle between line from the circle center to the
		   end point and the line perpendicular to the straight
		   waveguide. */
  double b0; /* Angle to rotate x0 */
  double x0 = cp.x;
  double y0 = cp.y;
  double a0 = cp.a;
  double dm, dp;
  double mm[2], mp[2];
  double p0[2], p1[2], pp[2];

  struct geom_al g;

  a0 = map_0_2pi(a0);

  /* Center of circle and end point. */
  m0[0] = x0 + r0 * sin(a0);
  m0[1] = y0 - r0 * cos(a0);
  p1[0] = x1;
  p1[1] = y1;

  dm = sqrt((m0[0]-p1[0])*(m0[0]-p1[0]) + (m0[1]-p1[1])*(m0[1]-p1[1]));
  
  if (fabs(r0) > dm) {
    fprintf(stderr, "arc_line: can't connect points.\n");
    fprintf(stderr, "  r0=%g x1=%g y1=%g\n", r0, x1, y1);
    exit(1);
  }

  mm[0] = (p1[0] - m0[0]) / dm;
  mm[1] = (p1[1] - m0[1]) / dm;
  alpha = -acos(r0 / dm);

  /* unit vertor from m to p. */
  mp[0] =  mm[0] * cos(alpha) + mm[1] * sin(alpha);
  mp[1] = -mm[0] * sin(alpha) + mm[1] * cos(alpha);

  /* Point at first circle. */
  p0[0] = m0[0] + r0 * mp[0];
  p0[1] = m0[1] + r0 * mp[1];

  dp = sqrt((p0[0]-p1[0])*(p0[0]-p1[0]) + (p0[1]-p1[1])*(p0[1]-p1[1]));

  /* Unit vector from P0 to P1 */
  pp[0] = (p1[0] - p0[0]) / dp;
  pp[1] = (p1[1] - p0[1]) / dp;

  /* Angle to rotate x0. */
  b0 = acos(pp[0]);
  if (pp[1] < 0) b0 = 2*M_PI - b0;
  b0 = r0 > 0 ? a0 - b0 : b0 - a0;
  b0 = map_0_2pi(b0);
  b0 = copysign(b0, r0);

  if (b0 > 0) { /* curve to the right */
    offset(o0);
    cw(fabs(r0)-o0, b0, w);
    offset(-o0);
  } else { /* curve to the left */
    offset(-o0);
    cw(fabs(r0)-o0, b0, w);
    offset(o0);
  }
  sw(dp, w);

  g.ang  = fabs(b0);
  g.line = dp;

  return g;
}

/**
 * Connect two points with arc-straight-arc. The direction of the arc in
 * each of the points is determined by the sign of the radius. When looking
 * in the direction of the point, a positive radius curves to the right and
 * a negative radius curves to the left.
 */
struct geom_ala arc_line_arc(double r0, double o0,
  double x1, double y1, double a1, double r1, double o1, double w)
{
  double m0[2]; /* Middle of circle through first point. */
  double m1[2]; /* Middle of circle through second point. */
  double alpha; /* Angle between line connecting the centers and line
		   perpendicular to the straight waveguide. */
  double b0; /* Angle to rotate x0 */
  double b1; /* Angle to rotate x1 */
  double x0 = cp.x;
  double y0 = cp.y;
  double a0 = cp.a;
  double dm, dp;
  double mm[2], mp[2];
  double p0[2], p1[2], pp[2];

  struct geom_ala g;

  a0 = map_0_2pi(a0);
  a1 = map_0_2pi(a1 + M_PI);

  /* Centers of circles through the points. */
  m0[0] = x0 + r0 * sin(a0);
  m0[1] = y0 - r0 * cos(a0);
  m1[0] = x1 - r1 * sin(a1);
  m1[1] = y1 + r1 * cos(a1);

  dm = sqrt((m0[0]-m1[0])*(m0[0]-m1[0]) + (m0[1]-m1[1])*(m0[1]-m1[1]));
  
  if (fabs(r0 - r1) > dm) {
    fprintf(stderr, "arc_line_arc: can't connect points.\n");
    fprintf(stderr, "  r0=%g x1=%g y1=%g a1=%g r1=%g\n", r0, x1, y1, a1, r1);
    exit(1);
  }

  mm[0] = (m1[0] - m0[0]) / dm;
  mm[1] = (m1[1] - m0[1]) / dm;
  alpha = -acos((r0 - r1) / dm);

  /* unit vertor from m to p. */
  mp[0] =  mm[0] * cos(alpha) + mm[1] * sin(alpha);
  mp[1] = -mm[0] * sin(alpha) + mm[1] * cos(alpha);

  /* Point at first circle. */
  p0[0] = m0[0] + r0 * mp[0];
  p0[1] = m0[1] + r0 * mp[1];

  /* Point at second circle. */
  p1[0] = m1[0] + r1 * mp[0];
  p1[1] = m1[1] + r1 * mp[1];

  dp = sqrt((p0[0]-p1[0])*(p0[0]-p1[0]) + (p0[1]-p1[1])*(p0[1]-p1[1]));

  /* Unit vector from P0 to P1 */
  pp[0] = (p1[0] - p0[0]) / dp;
  pp[1] = (p1[1] - p0[1]) / dp;

  /* Angle to rotate x0. */
  b0 = acos(pp[0]);
  if (pp[1] < 0) b0 = 2*M_PI - b0;
  b1 = b0 + M_PI;
  b0 = r0 > 0 ? a0 - b0 : b0 - a0;
  b0 = map_0_2pi(b0);
  b0 = copysign(b0, r0);

  /* Angle to rotate x1. */
  b1 = r1 > 0 ? b1 - a1 : a1 - b1;
  b1 = map_0_2pi(b1);
  b1 = copysign(b1, r1);

#ifdef DEBUG
  fprintf(stderr, "r0=%8.3f b0=%8.3f o0=%8.3f\n",
    r0*1e6, b0*180/M_PI, o0*1e6);
  fprintf(stderr, "r1=%8.3f b1=%8.3f o1=%8.3f\n",
    r1*1e6, b1*180/M_PI, o1*1e6);
#endif

  if (b0 > 0) { /* curve to the right */
    offset(o0);
    cw(fabs(r0)-o0, b0, w);
    offset(-o0);
  } else { /* curve to the left */
    offset(-o0);
    cw(fabs(r0)-o0, b0, w);
    offset(o0);
  }
  sw(dp, w);
  if (b1 > 0) { /* curve to the right */
    offset(o1);
    cw(fabs(r1)-o1, b1, w);
    offset(-o1);
  } else { /* curve to the left */
    offset(-o1);
    cw(fabs(r1)-o1, b1, w);
    offset(o1);
  }

  g.ang1 = fabs(b0);
  g.line = dp;
  g.ang2 = fabs(b1);

  return g;
}

/**
 * Connect two points with straight-arc-straight.
 */
struct geom_lal line_arc_line(double r, double o, double x1, double y1,
  double a1, double w)
{
  double x0 = cp.x;
  double y0 = cp.y;
  double a0 = cp.a;
  double l[2];
  double r0[2], r1[2], d;

  struct geom_lal g;

  a0 = map_0_2pi(a0);
  a1 = map_0_2pi(a1);

  /* directional vector at point 0 */
  r0[0] = cos(a0);
  r0[1] = sin(a0);
  /* directional vector at point 1 */
  r1[0] = cos(a1);
  r1[1] = sin(a1);

  /* coefficient determinant */
  d = r0[0]*r1[1] - r0[1]*r1[0];
  if (d == 0) {
    fprintf(stderr, "line_arc_line: lines are parallel\n");
    exit(1);
  }
  /* Calculate length of lines */
  l[0] = ((x1 - x0 + r * sin(a1) - r * sin(a0)) * r1[1] -
          (y1 - y0 - r * cos(a1) + r * cos(a0)) * r1[0]) / d;
  l[1] = ((y1 - y0 - r * cos(a1) + r * cos(a0)) * r0[0] -
          (x1 - x0 + r * sin(a1) - r * sin(a0)) * r0[1]) / d;
  
  if (l[0] < 0 || l[1] < 0) {
    fprintf(stderr, "line_arc_line: one or both straight lengths less than zero (%g, %g)\n", l[0], l[1]);
    exit(1);
  }
  sw(l[0],w);
  if (r > 0) { /* curve to the right */
    offset(o);
    cw(fabs(r)-o, fmod(a0-a1+2*M_PI, 2*M_PI), w);
    g.ang   = fabs(fmod(a0-a1+2*M_PI, 2*M_PI));
    offset(-o);
  } else { /* curve to the left */
    offset(-o);
    cw(fabs(r)-o, -fmod(a1-a0+2*M_PI, 2*M_PI), w);
    g.ang   = fabs(fmod(a1-a0+2*M_PI, 2*M_PI));
    offset(o);
  }
  sw(l[1],w);

  g.line1 = l[0];
  g.line2 = l[1];

  return g;
}

/* Sbend generates the curved waveguides in an S-bend calculating the
 * angles such that the displacement equals d. Offsets are applied both at
 * the start and at the end of the sections, so a typical use would be:
 * sw(l, w);
 * sbend(d, r, o, w);
 * sw(l, w);
 */
double sbend(double d, double r, double o, double w) {
  double a;
  int dir;

  /* The sign of d determines the direction. We want to keep the direction
   * of the offset, to allow for ofsets in the opposite direction. */
  dir = d > 0 ? 1 : -1;
  d = fabs(d);
  r = fabs(r);

  if (r < d/2) {
    fprintf(stderr, "Error: d exceeds 2 * (r+o)\n");
    fprintf(stderr, "  Called: sbend(%g, %g, %g, %g);", d, r, o, w);
    fprintf(stderr, "  Exiting...");
    exit(1);
  }
  a = acos(1-0.5* d/(r+o)) * dir;

  offset(o * dir);
  cw(r, a, w);
  offset(-2 * dir * o);
  cw(r, -a, w);
  offset(o * dir);
  return(fabs(2*(r+o)*sin(a)));
}

/**
 * This routine outputs two circular segments connecting the current point
 * to the requested position $(x, y, a)$ (absolute coordinates). The radius
 * of the first circle is given and the second follows from that. The
 * offset is accounted for in the calculation of the second radius.
 * The angles of both and the radius of the second circle is returned.
 * Figure~\ref{fig:2arc} shows the variables used.
 * \begin{figure}[tb]
 * \begin{center}
 *   \resizebox{60mm}{!}{\input{2arc.tex}}
 * \end{center}
 * \caption{Definition of variables.}
 * \label{fig:2arc}
 * \end{figure}
 */
int arc_arc(double R0, double x1, double y1, double a1,
  double offset, double *f, double *R1)
{
  /* f[0] and f[1] are the angles. */

  double r0[2], r1[2];		/* Directional vectors. */
  double a, b, cx[2];
  double c0[2], c1[2];		/* Circle centers. */
  double cpx, cpy, cpa;

  if (R0 > 0) {
    r0[0] = sin(cp.a);
    r0[1] = -cos(cp.a);
  } else {
    r0[0] = -sin(cp.a);
    r0[1] = cos(cp.a);
    R0 = -R0;
  }

  if (*R1 > 0) {
    r1[0] = sin(a1);
    r1[1] = -cos(a1);
  } else {
    r1[0] = -sin(a1);
    r1[1] = cos(a1);
  }

  c0[0] = cp.x + R0 * r0[0];
  c0[1] = cp.y + R0 * r0[1];

  cpx = cp.x; cpy = cp.y; cpa = cp.a;

  /*     __   __   __        __      __
   * Let cx = x1 - c0. From |cx + R1*r1| = R0 + R1 follows
   * a condition for R1: a * R1 = b.
   */
  cx[0] = x1 - c0[0];
  cx[1] = y1 - c0[1];
  a = 2 * (r1[0] * cx[0] + r1[1] * cx[1] - R0);
  b = R0 * R0 - cx[0] * cx[0] - cx[1] * cx[1];

  *R1 = b / a;

  c1[0] = x1 + *R1 * r1[0];
  c1[1] = y1 + *R1 * r1[1];

  /*
   * The angle follows from the inproduct between r0 and (c1-c0) and r1
   * with (c0-c1). The length of the vectors is one and (R0+R1),
   * respectively.
   */
  f[0] = acos((r0[0] * (c0[0] - c1[0]) + r0[1] * (c0[1] - c1[1])) /
    (R0 + *R1));
  f[1] = acos((r1[0] * (c1[0] - c0[0]) + r1[1] * (c1[1] - c0[1])) /
    (R0 + *R1));

  /*
  org(c0[0], c0[1], 0, NOFLIP);
  circle(1, R0);
  fprintf(stderr, "%f %f %f %f\n", c0[0], c0[1], R0, f[0]/M_PI*180);
  org(c1[0], c1[1], 0, NOFLIP);
  circle(1, *R1);
  fprintf(stderr, "%f %f %f %f\n", c1[0], c1[1], *R1, f[1]/M_PI*180);
  org(cpx, cpy, cpa, NOFLIP);
  */
  return(0);
}

